// --------------------------------------------------------------------------------------------------------------------
// <copyright file="GlobalMap.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Navigation.Localization
{
    using System;
    using System.Collections.Concurrent;
    using System.Collections.Generic;
    using System.Linq;
    using System.Runtime.CompilerServices;
    using System.Runtime.Serialization;
    using System.Threading.Tasks;
    using Microsoft.Robotics.Navigation.Perception;
    using Microsoft.Robotics.Numerics;
    using ShoNS.Stats;

    /// <summary>
    /// The GlobalMap class. It represents the world as an occupancy grid. 
    /// Each cell in the grid represents the probability that the corresponding area is occupied.
    /// The resolution of the cells (that is, the size of the area each cell represents) is stored as part of the map,
    /// as it is required for proper localization.
    /// Localization will generate one instance of this class for each particle.
    /// </summary>
    /// <remarks>
    /// For optimization purposes, the global map is internally represented as a set of tiles (mini-maps), and each tile maintains the actual cells. 
    /// This representation allows multiple maps to share the tiles that are identical. This representation has no bearing on the contract of the class.
    /// </remarks>
    [DataContract]
    public class GlobalMap
    {
        /// <summary>
        /// The probability value used by default for unexplored cells.
        /// </summary>
        public const float UnexploredCellProbability = 0.5f;

        /// <summary>
        /// The default distance resolution in meters.
        /// </summary>
        public const float DefaultDistanceResolution = 0.1f;

        /// <summary>
        /// The default width of the global map in pixel
        /// </summary>
        private const int DefaultWidth = 1024;

        /// <summary>
        /// The default height of the global map in pixel
        /// </summary>
        private const int DefaultHeight = 1024;

        /// <summary>
        /// The default observation cutoff depth.
        /// </summary>
        private const double DefaultMaximumObservationDepthInMeters = 30;

        /// <summary>
        /// The default position on the X axis where the robot starts, as a fraction of the map height
        /// </summary>
        private const double DefaultOriginXFactor = 0.25; ////0.42;

        /// <summary>
        /// The default position on the Y axis where the robot starts, as a fraction of the map width
        /// </summary>
        private const double DefaultOriginYFactor = 0.25; ////0.42;

        /// <summary>
        /// A mask used to compute the in-tile index
        /// </summary>
        private const byte TileIndexMask = (1 << MapTile.LogTileSize) - 1;

        /// <summary>
        /// The TSDF truncation threshold in cells
        /// </summary>
        private const float TruncationThresholdInCells = 4;

        /// <summary>
        /// The default size of cache buckets in cells (e.g. 5 means cache buckets of 5x5 map cells)
        /// </summary>
        private const double DefaultMapCacheResolution = 0.25;

        /// <summary>
        /// The default angular size of cache buckets 
        /// </summary>
        private const double DefaultMapCacheAngularResolution = 8 * MathConstants.Degrees2Radians;

        /// <summary>
        /// The internal cache of surface points for each visited pose
        /// </summary>
        private MapCache<IEnumerable<Point2D>> surfacePointCache;

        /// <summary>
        /// The distanceResolution for each pixel width in meters per pixels.
        /// </summary>
        [DataMember]
        private float distanceResolution;

        /// <summary>
        /// The inverse of the distanceResolution for each pixel width in meters per pixels.
        /// </summary>
        [DataMember]
        private double inverseDistanceResolution;

        /// <summary>
        /// The width of the global map in pixels
        /// </summary>
        [DataMember]
        private int width;

        /// <summary>
        /// The height of the global map in pixels
        /// </summary>
        [DataMember]
        private int height;

        /// <summary>
        /// The the number of tiles along the Y axis in the global map 
        /// </summary>
        [DataMember]
        private int widthInTiles;

        /// <summary>
        /// The the number of tiles along the X axis in the global map 
        /// </summary>
        [DataMember]
        private int heightInTiles;

        /// <summary>
        /// The position on the X axis where the robot starts
        /// </summary>
        [DataMember]
        private double originX;

        /// <summary>
        /// The position on the Y axis where the robot starts
        /// </summary>
        [DataMember]
        private double originY;

        /// <summary>
        /// The map is internally represented as an array of tiles, such that read-only tiles can be shared between multiple global maps.
        /// </summary>
        [DataMember]
        private MapTile[] metaMap;

        /// <summary>
        /// The last camera pose
        /// </summary>
        [DataMember]
        private Pose2D latestCameraPose;

        /// <summary>
        /// The last time the map was updated.
        /// </summary>
        [DataMember]
        private DateTime lastUpdateTime;

        /// <summary>
        /// The distance truncation threshold
        /// </summary>
        [DataMember]
        private float truncationThreshold;

        /// <summary>
        /// The observation cutoff depth.
        /// </summary>
        [DataMember]
        private double maximumObservationDepthInMeters;

        /// <summary>
        /// Random number generator
        /// </summary>
        private Random rand;

        /// <summary>
        /// Initializes a new instance of the GlobalMap class
        /// </summary>
        public GlobalMap()
            : this(DefaultWidth, DefaultHeight, new Pose2D(DefaultHeight * DefaultOriginXFactor, DefaultWidth * DefaultOriginYFactor, 0), DefaultDistanceResolution)
        {
        }

        /// <summary>
        /// Initializes a new instance of the GlobalMap class
        /// </summary>
        /// <param name="width">The desired width of the global map. This is the Y axis.</param>
        /// <param name="height">The desired height of the global map. This is the X axis.</param>
        /// <param name="startPose">The start pose of the robot on the map.</param>
        /// <param name="distanceResolution">
        /// The desired distance resolution in meters per pixel.
        /// The resolution is uniform across x and y axis.
        /// </param>
        /// <param name="maximumObservationDepthInMeters">The cutoff distance for observations.</param>
        public GlobalMap(int width, int height, Pose2D startPose, float distanceResolution = DefaultDistanceResolution, double maximumObservationDepthInMeters = DefaultMaximumObservationDepthInMeters)
        {
            int tileSize = 1 << MapTile.LogTileSize;
            this.distanceResolution = distanceResolution;
            this.inverseDistanceResolution = 1 / distanceResolution;
            this.widthInTiles = (width / tileSize) + ((width % tileSize > 0) ? 1 : 0);
            this.heightInTiles = (height / tileSize) + ((height % tileSize > 0) ? 1 : 0);

            this.width = this.widthInTiles * tileSize;
            this.height = this.heightInTiles * tileSize;
            this.metaMap = new MapTile[this.widthInTiles * this.heightInTiles];
            for (int i = 0; i < this.metaMap.Length; i++)
            {
                this.metaMap[i] = new MapTile();
#if DEBUG
                this.metaMap[i].Owner = this;
#endif
            }

            this.originX = startPose.X;
            this.originY = startPose.Y;
            this.latestCameraPose = new Pose2D(this.originX, this.originY, 0);
            this.rand = new Random();
            this.truncationThreshold = TruncationThresholdInCells * distanceResolution;
            this.maximumObservationDepthInMeters = maximumObservationDepthInMeters;
        }

        /// <summary>
        /// Gets the map distance resolution (in meters per pixel)
        /// </summary>
        public float DistanceResolution
        {
            get
            {
                return this.distanceResolution;
            }
        }

        /// <summary>
        /// Gets the map width (in pixel) 
        /// </summary>
        public int Width
        {
            get
            {
                return this.width;
            }
        }

        /// <summary>
        /// Gets the map height (in pixel) 
        /// </summary>
        public int Height
        {
            get
            {
                return this.height;
            }
        }

        /// <summary>
        /// Gets the camera pose
        /// </summary>
        public Pose2D Pose
        {
            get
            {
                return this.latestCameraPose;
            }
        }

        /// <summary>
        /// Gets or sets the robot start point on the map.
        /// </summary>
        public Pose2D MapStartPose
        {
            get 
            { 
                return new Pose2D(this.originX, this.originY, 0); 
            }

            set 
            { 
                this.originX = value.X; 
                this.originY = value.Y; 
            }
        }

        /// <summary>
        /// Gets the timestamp of the last map update.
        /// </summary>
        public DateTime LastUpdateTime
        {
            get { return this.lastUpdateTime; }
        }

        /// <summary>
        /// Gets the probability that the given cell is occupied. 
        /// This function exists to maintain backwards compatibility for Navigation code.
        /// </summary>
        /// <param name="x">The vertical coordinate.</param>
        /// <param name="y">The horizontal coordinate.</param>
        /// <returns>The probability that the cell is occupied (0 free, 1 occupied).</returns>
        public float this[int x, int y]
        {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            get
            {
                MapCell cell = this.GetCell(x, y);
                if (!cell.WasVisited)
                {
                    return UnexploredCellProbability; 
                }

                return UnexploredCellProbability + (UnexploredCellProbability - Math.Abs(cell.Value)) * (cell.Weight > 1 ? 1 : cell.Weight);
            }

            // for path-planning test cases only 
            internal set
            {
                if (value < UnexploredCellProbability)
                {
                    this.SetCell(x, y, this.truncationThreshold, value);
                }
                else if (value > UnexploredCellProbability)
                {
                    this.SetCell(x, y, 0, value);
                }
                else
                {
                    this.SetCell(x, y, 0, 0);
                }
            }
        }

        /// <summary>
        /// Returns the 2D camera pose in the global coordinate system, given the robot pose in the global coordinate system and the camera pose in a robot frame of reference
        /// </summary>
        /// <param name="robotPose">The pose of the robot</param>
        /// <param name="robotRelativeCameraPose">The pose of the camera relative to the robot</param>
        /// <returns>The 2D pose of the camera in the global system</returns>
        public static Pose2D GetCameraPoseInMapCoordinates(Pose2D robotPose, Pose2D robotRelativeCameraPose)
        {
            // TODO: clean this up to use transforms
            double localMapHeadingRotation = robotPose.Heading;
            double rotatedCameraX = robotRelativeCameraPose.X * Math.Cos(localMapHeadingRotation) - robotRelativeCameraPose.Y * Math.Sin(localMapHeadingRotation);
            double rotatedCameraY = robotRelativeCameraPose.X * Math.Sin(localMapHeadingRotation) + robotRelativeCameraPose.Y * Math.Cos(localMapHeadingRotation);

            // except for newPose, all other poses are in meters
            Pose2D rotatedCameraPose = new Pose2D(rotatedCameraX, rotatedCameraY, robotRelativeCameraPose.Heading);
            Pose2D newPose = new Pose2D(robotPose.X + rotatedCameraPose.X, robotPose.Y + rotatedCameraPose.Y, localMapHeadingRotation + rotatedCameraPose.Heading);
            return newPose;
        }

        /// <summary>
        /// Returns the value of the given cell. 
        /// </summary>
        /// <param name="x">The vertical coordinate.</param>
        /// <param name="y">The horizontal coordinate.</param>
        /// <returns>The value of the cell.</returns>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public MapCell GetCell(int x, int y)
        {
            int tileY = y >> MapTile.LogTileSize;
            int tileX = x >> MapTile.LogTileSize;
            int inTileY = y & TileIndexMask;
            int inTileX = x & TileIndexMask;

            int tileIndex = (tileX * this.widthInTiles) + tileY;
            MapTile tile = this.metaMap[tileIndex];
            return tile[inTileX, inTileY];
        }

        /// <summary>
        /// Sets the value of the cell.
        /// </summary>
        /// <param name="x">The vertical coordinate.</param>
        /// <param name="y">The horizontal coordinate.</param>
        /// <param name="value">The new value to set the cell to</param>
        /// <param name="weight">The weight of the cell</param>
        /// <param name="metadata">Additional information about the cell, e.g. whether the pixel is in an exclusion zone</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void SetCell(int x, int y, float value, float weight, float metadata = 0)
        {
            int tileY = y >> MapTile.LogTileSize;
            int tileX = x >> MapTile.LogTileSize;
            int inTileY = y & TileIndexMask;
            int inTileX = x & TileIndexMask;

            int tileIndex = (tileX * this.widthInTiles) + tileY;
            MapTile tile = this.metaMap[tileIndex];

            if (!tile.IsWritable)
            {
                // copy the tile to take ownership
                tile = tile.Clone();
#if DEBUG
                tile.Owner = this;
#endif
                this.metaMap[tileIndex] = tile;
            }

#if DEBUG
            if (this != tile.Owner)
            {
                throw new Exception("Unprotected write detected");
            }
#endif
            this.metaMap[tileIndex][inTileX, inTileY] = new MapCell(value, weight, metadata);
        }

        /// <summary>
        /// Returns true if the coordinates identify free space
        /// </summary>
        /// <param name="x">The x coordinate</param>
        /// <param name="y">The y coordinate</param>
        /// <returns>True if the map is free at the specified location</returns>
        public bool IsFree(int x, int y)
        {
            var cell = this.GetCell(x, y);
            return cell.Weight * cell.Value >= 1;
        }

        /// <summary>
        /// Convert meters to map cells at this map's resolution.
        /// </summary>
        /// <param name="meters">Value in meter units.</param>
        /// <returns>Value in map cell units.</returns>
        public double MetersToMapCells(double meters)
        {
            return meters * this.inverseDistanceResolution;
        }

        /// <summary>
        /// Convert meters to map cells at this map's resolution.
        /// </summary>
        /// <param name="pose">Pose in meters.</param>
        /// <returns>Value in map cell units.</returns>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public Pose2D MetersToMapCells(Pose2D pose)
        {
            return new Pose2D(pose.X * this.inverseDistanceResolution, pose.Y * this.inverseDistanceResolution, pose.Heading);
        }

        /// <summary>
        /// Convert map cells to meters at this map's resolution.
        /// </summary>
        /// <param name="cells">Value in map cell units.</param>
        /// <returns>Value in meter units.</returns>
        public double MapCellsToMeters(double cells)
        {
            return cells * this.distanceResolution;
        }

        /// <summary>
        /// Convert map cells to meters at this map's resolution.
        /// </summary>
        /// <param name="pose">Pose in map cell units.</param>
        /// <returns>Value in meter units.</returns>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public Pose2D MapCellsToMeters(Pose2D pose)
        {
            return new Pose2D(pose.X * this.distanceResolution, pose.Y * this.distanceResolution, pose.Heading);
        }

        /// <summary>
        /// Creates a writable copy of this global map. 
        /// </summary>
        /// <returns>A new instance that can be written to without affecting its parent</returns>
        public GlobalMap MakeWritableCopy()
        {
            GlobalMap newMap = new GlobalMap(this.width, this.height, this.MapStartPose, this.distanceResolution);
            this.CopyTo(newMap);
            
            return newMap;
        }

        /// <summary>
        /// Enables caching of re-projected surfaces. SLAM must not call this, since the cache is invalidated on every update anyway.
        /// </summary>
        public void EnableCaching()
        {
            this.surfacePointCache = new MapCache<IEnumerable<Point2D>>(DefaultMapCacheResolution, DefaultMapCacheAngularResolution);
        }

        /// <summary>
        /// Copies the contents of this map to another map object, resulting in a shallow clone where references to the map tiles are copied, but the map tiles themselves are not.
        /// </summary>
        /// <param name="newMap">A pre-allocated map to copy to</param>
        public void CopyTo(GlobalMap newMap)
        {
            newMap.latestCameraPose = this.latestCameraPose;
            newMap.originX = this.originX;
            newMap.originY = this.originY;

            newMap.metaMap = (MapTile[])this.metaMap.Clone();

            int tileCount = newMap.metaMap.Length;
            for (int i = 0; i < tileCount; i++)
            {
                this.metaMap[i].IsWritable = false;
                newMap.metaMap[i].IsWritable = false;
            }
        }

        /// <summary>
        /// Update the global map by stitching the most recent piece of local map
        /// </summary>
        /// <param name="observation">The local map to fit into the global map.</param>
        /// <param name="newPose">The new position of the robot when the local map was generated</param>
        /// <param name="maximumDistance">The maximum distance to update</param>
        /// <param name="decayStart">The distance in meters up to which we fully trust the measurement. The update decays past this point with the inverse of the distance</param>
        /// <param name="dampeningFactor">
        /// A reduction factor to apply when updating the map. This limits the amount of change resulting from this update. 
        /// As long as all calls to Update use the same value, this only influences visualization (acting as a saturation factor when using a threshold of weight=1).
        /// Neutral value is 1.
        /// </param>
        /// <param name="erase">If true, the observation is removed from the map (reverse update). Defaults to false.</param>
        public void Update(IHorizontalDepthProfile observation, Pose2D newPose, double maximumDistance = 0, float decayStart = 0, float dampeningFactor = 1f, bool erase = false)
        {
            // transform between global coordinates and camera-centric coordinates 
            this.latestCameraPose = GlobalMap.GetCameraPoseInMapCoordinates(newPose, this.MetersToMapCells(observation.Pose));
            CoordinateTransform2D transformToGlobal = new CoordinateTransform2D(this.latestCameraPose, this.inverseDistanceResolution);
            CoordinateTransform2D transformToLocal = transformToGlobal.Reverse();

            // find every cell that could overlap with the current observation and update it with the observed distance along the corresponding ray
            double maxObservedDistance = 0;
            int count = observation.Length;
            for (int i = 0; i < count; i++)
            {
                double distance = observation.GetHitDistance(i);
                if (distance > maxObservedDistance)
                {
                    maxObservedDistance = distance;
                }
            }

            int maximumObservationDepthInCells = (int)this.MetersToMapCells(maximumDistance > 0 ? maximumDistance : maxObservedDistance * (1 + this.truncationThreshold));
            InclusionZoneMarker map = new InclusionZoneMarker();
            List<FastTuple<int, int, int>> inclusionZone = map.GenerateInclusionZoneScanLines(this.latestCameraPose, observation.FOV, maximumObservationDepthInCells);

            ////Parallel.ForEach(
            ////    inclusionZone,
            ////    t =>
            foreach (FastTuple<int, int, int> t in inclusionZone)
            {
                int y = t.Item3;
                if (y >= 0 && y < this.width)
                {
                    // visit every point in the rectangle and update it with the observed distance along the corresponding ray
                    for (int x = Math.Max(t.Item1 - 1, 0); x < Math.Min(t.Item2 + 1, this.height); x++)
                    {
                        // project the point into local coordinates
                        Point2D localPoint = transformToLocal.Transform(x, y);

                        // find the distance to the closest obstacle (in the HDP) along the same ray
                        double distanceToPoint;
                        int rayIndex = observation.Reproject(localPoint, out distanceToPoint);
                        if (rayIndex < 0)
                        {
                            // not in the field of view
                            continue;
                        }

                        double distanceToObstacle = observation.GetHitDistance(rayIndex);
                        if (distanceToObstacle == 0)
                        {
                            // no observation for the current point
                            continue;
                        }

                        double distanceFromPointToObstacle = distanceToObstacle - distanceToPoint;
                        double scaledTruncationThreshold = this.truncationThreshold * Math.Max(1, distanceToObstacle);

                        // We only process the point if:
                        // - is not too far beyond the observed obstacle and we can't update it without potentially corrupting observations made from the other side
                        // - the depth profile has valid depth data for this ray
                        // - the point falls inside the field of view
                        if (distanceFromPointToObstacle >= -scaledTruncationThreshold)
                        {
                            // get the current cell
                            MapCell cell = this.GetCell(x, y);

                            // the weight of the update is a function of distance to point. 
                            double adjustedDistance = distanceToPoint < decayStart ? 1 : 1 / (distanceToPoint - decayStart + 1);
                            double weight = adjustedDistance * adjustedDistance;

                            // the weight is also a function of the change in perspective since the last observation
                            // we divide the sensor in sub-sections and peform a reduced update if the section has not changed.
                            double newPerspective = (int)(rayIndex * 180 / count) + 1;
                            if (cell.Metadata == newPerspective)
                            {
                                weight = weight * weight;
                            }

                            // We also reduce the weight by the specified dampening factor.
                            weight = weight * dampeningFactor;

                            // the value of the update is the truncated distance to the obstacle, scaled for the distance to the camera
                            double delta = 1;
                            if (distanceFromPointToObstacle < scaledTruncationThreshold)
                            {
                                delta = distanceFromPointToObstacle / scaledTruncationThreshold;
                            }

                            // update the map.
                            int sign = 1;
                            if (erase)
                            {
                                sign = -1;
                            }

                            double newValue = (cell.Value * cell.Weight + sign * delta * weight) / (cell.Weight + sign * weight);
                            this.SetCell(x, y, (float)newValue, (float)(cell.Weight + sign * weight), (float)newPerspective);
                        }
                    }
                }
            }
                
            // update the timestamp
            this.lastUpdateTime = DateTime.Now;

            // clear the cache if caching is enabled 
            if (this.surfacePointCache != null)
            {
                this.surfacePointCache.Clear();
            }
        }
        
        /// <summary>
        /// Approximates the match quality between sensor data and model given the transform between the two, with cell-level quantization and by only looking at hit points.
        /// The cost function is flat with a big and narrow dip in the immediate vicinity of the correct pose, and thus can only be used in tracking with high frequency. 
        /// It cannot be used to decide between the better of two incorrect poses.
        /// </summary>
        /// <param name="observation">The new observation to match</param>
        /// <param name="newPose">The proposed pose of the observation</param>
        /// <returns>The sum of TSDF deltas for each occupied pixel in the observation</returns>
        public double ApproximateMatch(IHorizontalDepthProfile observation, Pose2D newPose)
        {
            // get the camera pose in global coordinates
            Pose2D cameraPose = GlobalMap.GetCameraPoseInMapCoordinates(newPose, this.MetersToMapCells(observation.Pose));

            // transform between camera-centric coordinates and global coordinates
            CoordinateTransform2D transformToGlobal = new CoordinateTransform2D(cameraPose, this.inverseDistanceResolution);

            double penalty = 0;
            int count = observation.Length;
            for (int i = 0; i < count; i++)
            {
                double distanceToPoint = observation.GetHitDistance(i);
                if (distanceToPoint == 0)
                {
                    continue;
                }

                // transform the point to map coordinates and make sure it is on the map
                Point2D localPoint = observation.GetHitPoint(i);
                Point2D pointOnMap = transformToGlobal.Transform(localPoint);
                int px = (int)(pointOnMap.X + 0.5);
                int py = (int)(pointOnMap.Y + 0.5);
                if (px < 0 || py < 0 || px >= this.height || py >= this.width)
                {
                    continue;
                }

                // get the value and weight of the map entry. The lower the weight, the less important any discrepancy
                MapCell cell = this.GetCell(px, py);
                double distance = 0; 
                if (cell.WasVisited)
                {
                    distance = (cell.WasVisited ? cell.Value : -1) * this.truncationThreshold * distanceToPoint;
                }

                penalty += distance * distance;
            }

            return -penalty;
        }

        /// <summary>
        /// Matches an observation to the map given a pose, and returns the likelihood that the pose is correct
        /// </summary>
        /// <param name="observation">The new observation to match</param>
        /// <param name="newPose">The proposed pose of the observation</param>
        /// <param name="allowExploration">If false, matching against unexplored areas is penalized. </param>
        /// <param name="surface">The re-projected surface as an HDP</param>
        /// <returns>The likelihood that the pose is correct given the map and the observation</returns>
        public double Match(IHorizontalDepthProfile observation, ref Pose2D newPose, bool allowExploration, out IHorizontalDepthProfile surface)
        {
            // get the camera pose in global coordinates
            Pose2D cameraPose = GlobalMap.GetCameraPoseInMapCoordinates(newPose, this.MetersToMapCells(observation.Pose));

            // if caching is enabled, check whether we have a cached surface.
            IEnumerable<Point2D> surfacePoints;
            if (this.surfacePointCache != null)
            {
                surfacePoints = this.GetCachedSurfacePoints(cameraPose, observation.FOV);
            }
            else
            {
                surfacePoints = this.GetEstimatedSurfacePoints(cameraPose, observation.FOV, this.maximumObservationDepthInMeters);
            }

            // measure and return the error given the set of surface points
            double error = this.MatchObservationToSurfacePoints(observation, cameraPose, surfacePoints, allowExploration, out surface);
            return error;
        }

        /// <summary>
        /// Gradient descent search for best match
        /// </summary>
        /// <param name="observation">The observation to match</param>
        /// <param name="newPose">The starting pose</param>
        /// <param name="allowExploration">If true, exploration of unknown space is not penalized</param>
        /// <param name="surface">The resulting surface from ray tracing</param>
        /// <param name="currentError">The current best error (higher is better)</param>
        /// <param name="iterationDepth">The current iteration depth</param>
        /// <returns>The smallest error found</returns>
        public double GradientDescentMatch(IHorizontalDepthProfile observation, ref Pose2D newPose, bool allowExploration, ref IHorizontalDepthProfile surface, double currentError, int iterationDepth)
        {
            double translationStep = 0.002;
            double rotationStep = MathConstants.Degrees2Radians / 10;
            Pose2D[] neighborIncrements = new[] 
            {
                new Pose2D(translationStep, 0, 0), 
                new Pose2D(-translationStep, 0, 0), 
                new Pose2D(0, translationStep, 0), 
                new Pose2D(0, -translationStep, 0), 
                new Pose2D(0, 0, rotationStep), 
                new Pose2D(0, 0, -rotationStep)
            };

            bool localMinimum = true;
            for (int i = 0; i < neighborIncrements.Length; i++)
            {
                Pose2D altPose = newPose + neighborIncrements[i];
                IHorizontalDepthProfile altSurface;
                double altError = this.Match(observation, ref altPose, allowExploration, out altSurface);
                if (altError > currentError)
                {
                    currentError = altError;
                    surface = altSurface;
                    newPose = altPose;
                    localMinimum = false;
                }
            }

            iterationDepth--;
            if (iterationDepth == 0 || localMinimum)
            {
                return currentError;
            }

            return this.GradientDescentMatch(observation, ref newPose, allowExploration, ref surface, currentError, iterationDepth);
        }

          /// <summary>
        /// Determines the surface as seen from the specified pose (by ray-tracing or by using cached surface points) and returns the points describing it.
        /// </summary>
        /// <param name="cameraPose">The position and orientation of the observer, in map cells</param>
        /// <param name="fieldOfView">The desired field of view to return</param>
        /// <returns>The set of occupied points defining the surrounding visible surface, in the local coordinate frame defined by the pose</returns>
        private IEnumerable<Point2D> GetCachedSurfacePoints(Pose2D cameraPose, double fieldOfView)
        {
            Tuple<Pose2D, IEnumerable<Point2D>> surfaceDefinition;
            surfaceDefinition = this.surfacePointCache.GetOrAdd(
                cameraPose,
                p =>
                {
                    // surface is not in the cache, so generate it
                    // note that we generate a 360 view, not just the observation FOV view, so there is overhead implied by caching
                    return this.GetEstimatedSurfacePoints(p, fieldOfView + 2 * DefaultMapCacheAngularResolution, this.maximumObservationDepthInMeters);
                });

            // we got a definition from the cache, 
            // trasform the points from one camera pose to the other
            Pose2D newCameraPose = this.MapCellsToMeters(cameraPose);
            Pose2D oldCameraPose = this.MapCellsToMeters(surfaceDefinition.Item1);
            IEnumerable<Point2D> surfacePoints = this.TransformSurfacePointsToLocalCoordinates(surfaceDefinition.Item2, oldCameraPose, newCameraPose);
            return surfacePoints;
        }

        /// <summary>
        /// Determines the surface as seen from the specified pose (by ray-tracing) and returns the points describing it.
        /// </summary>
        /// <param name="pose">The position and orientation of the observer, in map cells</param>
        /// <param name="fieldOfView">The field of view to visit</param>
        /// <param name="maximumDepth">The maximum depth to visit</param>
        /// <returns>The set of occupied points defining the visible surface in the field of view, in the local coordinate frame defined by the pose</returns>
        private IEnumerable<Point2D> GetEstimatedSurfacePoints(Pose2D pose, double fieldOfView, double maximumDepth)
        {
            // Start with the left-most ray and rotate clockwise
            // the angular step is based on the angle bracketing a map cell at maxDepth distance. Because its a small angle, angle == sin(angle)
            double start = -fieldOfView / 2;
            double increment = MathConstants.Degrees2Radians / 15; //// TODO: this should be derived from hdp resolution, to be a little bit less than the smallest angle in the hdp
            int count = (int)(fieldOfView / increment) + 1;

            // would be nice to cache this array
            Point2D[] points = new Point2D[count];

            double angle = start;
            for (int i = 0; i < count; i++, angle += increment)
                {
                    // ray-trace from the given pose up to the specified distance, covering the specified field of view
                    // make sure there is a measurement (could be missing because this area of the map is seen for the first time)
                    double surfaceDepth;
                    if (this.RayTrace(pose, angle, maximumDepth, out surfaceDepth))
                    {
                        // add the hit point to the list of points (could be fake point representing a distance greater than maximumDepth)
                        double x = surfaceDepth * Math.Cos(angle);
                        double y = surfaceDepth * Math.Sin(angle);
                        points[i] = new Point2D(x, y);
                    }
            }

            return points;
        }

        /// <summary>
        /// Transforms the points from one camera pose to the other
        /// </summary>
        /// <param name="surface">The points to transform</param>
        /// <param name="oldCameraPose">The old camera pose (in meters)</param>
        /// <param name="newCameraPose">The new camera pose (in meters)</param>
        /// <returns>The transformed surface</returns>
        private IEnumerable<Point2D> TransformSurfacePointsToLocalCoordinates(IEnumerable<Point2D> surface, Pose2D oldCameraPose, Pose2D newCameraPose)
        {
            // transform the points from one camera pose to the other
            CoordinateTransform2D transformFromCachedToGlobal = new CoordinateTransform2D(oldCameraPose);
            CoordinateTransform2D transformFromGlobalToLocal = new CoordinateTransform2D(newCameraPose).Reverse();
            CoordinateTransform2D transformFromCachedToLocal = transformFromCachedToGlobal.Append(transformFromGlobalToLocal);
            return surface.Where(p => p.X != 0).Select(p => transformFromCachedToLocal.Transform(p));
        }

        /// <summary>
        /// Match a given observation to a set of surface points, returning the matching error
        /// </summary>
        /// <param name="observation">The observation to compare withe the surface points.</param>
        /// <param name="cameraPose">The camera pose in map coordinates, obtained by combining the new robot pose and the camera pose in robot coordinates</param>
        /// <param name="surfacePoints">The points making up the surface visible on the map from (approximately) the given pose. The points are local coordinates. The surface was computed in a prior step (or offline)</param>
        /// <param name="allowExploration">If false, matching against unexplored areas is penalized. </param>
        /// <param name="surface">The surface as an HDP</param>
        /// <returns>The log probability that the transformation is correct given the observation and the existing global map</returns>
        private double MatchObservationToSurfacePoints(IHorizontalDepthProfile observation, Pose2D cameraPose, IEnumerable<Point2D> surfacePoints, bool allowExploration, out IHorizontalDepthProfile surface)
        {
            // generate a re-projected HDP from the surface points and compare it with the observation
            IHorizontalDepthProfile reprojectedSurface = observation.Create();
            foreach (Point2D point in surfacePoints)
            {
                reprojectedSurface.AddHitPoint(point);
            }

            double error = this.MatchObservationToSurface(observation, reprojectedSurface, cameraPose, allowExploration);
            surface = reprojectedSurface;
            return error;
        }

        /// <summary>
        /// Match a given observation to a re-projected depth profile obtained from the map, returning the matching error 
        /// </summary>
        /// <param name="observation">The observation to compare with the re-projected surface.</param>
        /// <param name="surface">The surface visible on the map from the same pose as the observation.</param>
        /// <param name="cameraPose">The new position of the camera when the observation was generated</param>
        /// <param name="allowExploration">If false, matching against unexplored areas is penalized. </param>
        /// <returns>The log probability that the transformation is correct given the observation and the existing global map</returns>
        private double MatchObservationToSurface(IHorizontalDepthProfile observation, IHorizontalDepthProfile surface, Pose2D cameraPose, bool allowExploration)
        {
            int count = observation.Length;
            double penalty = 0;
            for (int i = 0; i < count; i++)
            {
                Point2D surfacePoint = surface.GetHitPoint(i);
                Point2D observationPoint = observation.GetHitPoint(i);
                double distanceToSurface = surface.GetHitDistance(i);
                double distanceToObservation = observation.GetHitDistance(i);

                // if observation or surface is a no-reading, skip it
                if ((observationPoint.X == 0) || (surfacePoint.X == 0 && allowExploration))
                {
                    continue;
                }

                // compute the point-to-point distance from observation to surface
                double distance = (distanceToSurface - distanceToObservation);
                penalty += distance * distance;
            }

            return -penalty;
        }

        /// <summary>
        /// Finds and returns the surface boundary along a given ray
        /// </summary>
        /// <param name="pose">The pose to trace from, in cells</param>
        /// <param name="angle">The angle of the ray relative to the pose</param>
        /// <param name="maximumDepth">The cutoff depth for ray tracing</param>
        /// <param name="depth">The distance to the surface boundary, if one is met, or a value greater than maximumDepth otherwise</param>
        /// <returns>True if a surface boundary was found, false otherwise</returns>
        private bool RayTrace(Pose2D pose, double angle, double maximumDepth, out double depth)
        {
            double increment = this.distanceResolution; 
            double lastPositiveValue = 0;
            double sin = Math.Sin(angle + pose.Heading);
            double cos = Math.Cos(angle + pose.Heading);
            bool positiveToNegative = false;
            
            depth = 2 * this.distanceResolution;
            
            // march towards the zero-crossing and interpolate the zero-crossing from the two values on each side
            maximumDepth += this.truncationThreshold;
            while (depth <= maximumDepth)
            {
                double depthInCells = this.MetersToMapCells(depth);
                double px = depthInCells * cos + pose.X;
                double py = depthInCells * sin + pose.Y;
                int x = (int)(px + 0.5);
                int y = (int)(py + 0.5);

                // are we out of bounds?
                if (x < 0 || y < 0 || x >= this.height - 1 || y >= this.width - 1)
                {
                    return false;
                }

                MapCell cell = this.GetCell(x, y);
                if (cell.Weight == 0)
                {
                    return false;
                }

                double value = cell.Value;
                if (value < 1)
                {
                    // interpolate the value (bilinear)
                    MapCell cell12 = this.GetCell(x, y + 1);
                    MapCell cell21 = this.GetCell(x + 1, y);
                    MapCell cell22 = this.GetCell(x + 1, y + 1);
                    value = value * (x + 1 - px) * (y + 1 - py) + cell12.Value * (x + 1 - px) * (py - y) + cell21.Value * (px - x) * (y + 1 - py) + cell22.Value * (px - x) * (py - y);

                    if (value < 0)
                    {
                        if (!positiveToNegative)
                        {
                            return false;
                        }

                        // we found the zero-crossing. Interpolate the position of the surface crossing from the two values we have on each side
                        depth = depth - this.distanceResolution * value / (value - lastPositiveValue);
                        return true;
                    }
                    else
                    {
                        // we are getting close to the surface on the positive side
                        lastPositiveValue = value;
                        positiveToNegative = true;
                    }
                }

                depth = depth + increment;
            }

            return false;
        }
    }
}
